#include "palnpath.hpp" 

void plan(void)
{
// 定义路径点 
nav_msgs::msg::Path path;
path.poses.resize(3); 
path.poses[0].pose.position.x  = 0.0;  // 起点 
path.poses[0].pose.position.y  = 0.0;
path.poses[1].pose.position.x  = 2.0;  // 中间点 
path.poses[1].pose.position.y  = 1.0;
path.poses[2].pose.position.x  = 4.0;  // 终点 
path.poses[2].pose.position.y  = 0.0;
 
// 计算目标点（根据前瞻距离选择路径点）
double lookahead_distance = 0.5;
size_t target_index = 0;
for (size_t i = 0; i < path.poses.size();  ++i) {
    double dx = path.poses[i].pose.position.x  - current_x;
    double dy = path.poses[i].pose.position.y  - current_y;
    if (std::hypot(dx, dy) >= lookahead_distance) {
        target_index = i;
        break;
        }
    }

// 计算目标方向角 
double target_yaw = std::atan2(
    path.poses[target_index].pose.position.y  - current_y,
    path.poses[target_index].pose.position.x  - current_x 
);
 
// 计算角速度（控制转向）
double angular_z = 0.5 * (target_yaw - current_yaw);
 
// 计算线速度（根据距离调整）
double linear_x = 0.2 * std::hypot(
    path.poses[target_index].pose.position.x  - current_x,
    path.poses[target_index].pose.position.y  - current_y 
);
}